
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       Imu.c
  * @author     baiyang
  * @date       2021-7-14
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "Imu.h"

/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
double math_rand_normal(double mean, double stddev);
//初始化变量  配置参数
void ImuMode_SetParam(ImuMode* pImu, float weight, float rpm_max, float gain, float num, Vector3f_t* scale, Vector3f_t* base, Vector3f_t* variance)
{
    //设置震动参数
    pImu->tVib.Weight = weight;
    pImu->tVib.Gain = gain;
    pImu->tVib.RPM_Max = rpm_max;
    pImu->num = num;

    pImu->Integtrator = (float *)malloc(sizeof(float) * pImu->num);
    memset(pImu->Integtrator, 0, sizeof(float) * pImu->num);

    lpf_set_cutoff1_vec2f(&(pImu->imu_filt), 50.0f);    //50HZ
    pImu->scale_error = *scale;
    pImu->base_error = *base;
    pImu->variance = *variance;


    pImu->enable_vib = true;
    pImu->enable_err = true;
}


void ImuMode_Update(ImuMode* pImu, Vector3f_t* pState_out, const Vector3f_t* pState_in, float* prop_vel, float dt)
{
    //计算螺旋桨震动
    Vector3f_t vib_value;    //三轴震动量
    vec3_zero(&vib_value);

    //如果使能震动模型
    if (pImu->enable_vib)
    {
        for (uint8_t cnt = 0; cnt < pImu->num; cnt++)
        {
            pImu->Integtrator[cnt] += prop_vel[cnt] * dt;
            float sin_rot, cos_rot;
            sin_rot = sinf(pImu->Integtrator[cnt]);
            cos_rot = cosf(pImu->Integtrator[cnt]);

            //计算权重
            float ratio;
            ratio = SQ(prop_vel[cnt] / (pImu->tVib.RPM_Max * RPM_TO_RPS));

            vib_value.x += ratio * sin_rot * pImu->tVib.Weight;
            vib_value.y += ratio * cos_rot * pImu->tVib.Weight;
        }

        vib_value.z = 0.5f * (vib_value.x + vib_value.y);
        vec3_mult(&vib_value, &vib_value, (pImu->tVib.Gain / 4.0f));
    }
    
    Vector3f_t state_vib, state_err;
    vec3_add(&state_vib, pState_in, &vib_value);

    if (pImu->enable_err)
    {
        //传感器偏差模型
        vec3_hadamard_product(&state_err, &state_vib, &(pImu->scale_error));
        vec3_add(&state_err, &state_err, &(pImu->base_error));

        //加上滤波器和白噪声干扰
        Vector3f_t filt_out = lpf_apply2_vec3f(&(pImu->imu_filt), &state_err, dt);

        //加上噪声干扰
        Vector3f_t noise;
        vec3_set(&noise, math_rand_normal(0, pImu->variance.x), math_rand_normal(0, pImu->variance.y), math_rand_normal(0, pImu->variance.z));
        vec3_add(pState_out, &filt_out, &noise);
    }
    else
    {
        *pState_out = state_vib;
    }

}

/*------------------------------------test------------------------------------*/


